#include "jump.h"
float start_time_ = 0.0f;

float jump_angle = 70;

float jump_extension = 26.8f; // 最大伸腿长度 [cm]

//void TrajectoryJump(float t, float launchTime, float stanceHeight, float downAMP) {
//    //Need to check if n works
//    float n = t/launchTime;
//    x = 0;
//    y = downAMP*n + stanceHeight;
//    //y = downAMP*sin(PI/4 + PI/4*n) + stanceHeight;
//}

//void StartJump(float start_time_s) {
//    start_time_ = start_time_s;
//    state = JUMP;
//}
extern int SpeedLimit;
float JumpStartTime = 0.0f;
void ExecuteJump(void) {
    // min radius = 0.8
    // max radius = 0.25

    const float prep_time = 0.8f; // 准备时间 [s]		0.8
    const float launch_time = 0.20f ; // 收缩腿前的持续时间 [s]		0.2
    const float fall_time = 0.3f; //收缩腿后持续时间恢复正常行为 [s]		0.8
	  const float streching_time = 0.3f;
	  const float wait_time = 0.3f;

    const float stance_height = 10.7f; // 跳跃之前腿的高度 [cm]  14.2
    const float fall_extension = 11.3f; // 降落时的期望腿长 [cm]


		float tt=times*5/1000-JumpStartTime;
    if (tt < prep_time) {

          x = -stance_height*cos(jump_angle*PI/180);		
        y = stance_height*sin(jump_angle*PI/180);
//			  SpeedLimit = 0;

        CartesianToTheta(1.0);
        // 使用刚度小，阻尼大的增益
				ChangeTheGainOfPID(8,0.1,70,0.22);
			  SetCoupledPosition(0);
			  SetCoupledPosition(1);
			  SetCoupledPosition(2);
			  SetCoupledPosition(3);
			
    } else if (tt >= prep_time && tt < prep_time + launch_time) {

//        x = -jump_extension*sin(jump_angle*PI/180);
			SpeedLimit = 8000;
				x = -jump_extension*cos(jump_angle*PI/180);
        y =  jump_extension*sin(jump_angle*PI/180);

        CartesianToTheta(1.0);

        // 使用高刚度和低阻尼执行跳转
				ChangeTheGainOfPID(8,0.0f,200,0.0f);
			  SetCoupledPosition(0);
			  SetCoupledPosition(1);
			  SetCoupledPosition(2);
			  SetCoupledPosition(3);

    } else if (tt >= prep_time + launch_time && tt < prep_time + launch_time + fall_time) {

				x = 17.3*cos((jump_angle+15)*PI/180);
        y = 17.3*sin((jump_angle+15)*PI/180);

        CartesianToTheta(1.0);
        //使用低刚度和大量的阻尼来处理下降
				ChangeTheGainOfPID(8,0.1,150,0);
			  SetCoupledPosition(0);
			  SetCoupledPosition(1);
			  SetCoupledPosition(2);
			  SetCoupledPosition(3);

    } else if (tt >= prep_time + launch_time + fall_time && tt < prep_time + launch_time + fall_time + streching_time) {
			
				x = 17.3*cos((jump_angle+15)*PI/180);
        y = 17.3*sin((jump_angle+15)*PI/180);

        CartesianToTheta(1.0);
        //使用低刚度和大量的阻尼来处理下降
			ChangeTheGainOfPID(8,0.1,2.0f,0.1f);
			  SetCoupledPosition(0);
			  SetCoupledPosition(1);
			  SetCoupledPosition(2);
			  SetCoupledPosition(3);
		
		}
		else {
			
				state = REALSE;
				ChangeTheGainOfPID(8,0.1,70,0.22);
    }

}





void ExecuteLittleJump(void) {
    // min radius = 0.8
    // max radius = 0.25

    const float prep_time = 0.8f; // 准备时间 [s]		0.8
    const float launch_time = 0.20f ; // 收缩腿前的持续时间 [s]		0.2
    const float fall_time = 0.3f; //收缩腿后持续时间恢复正常行为 [s]		0.8
	  const float streching_time = 0.3f;
	  const float wait_time = 0.3f;

    const float stance_height = 13.7f; // 跳跃之前腿的高度 [cm]  14.2
    const float fall_extension = 11.3f; // 降落时的期望腿长 [cm]


		float tt=times*5/1000-JumpStartTime;
    if (tt < prep_time) {

          x = -stance_height*cos(jump_angle*PI/180);		
        y = stance_height*sin(jump_angle*PI/180);
//			  SpeedLimit = 0;

        CartesianToTheta(1.0);
        // 使用刚度小，阻尼大的增益
				ChangeTheGainOfPID(8,0.1,70,0.22);
			  SetCoupledPosition(0);
			  SetCoupledPosition(1);
			  SetCoupledPosition(2);
			  SetCoupledPosition(3);
			
    } else if (tt >= prep_time && tt < prep_time + launch_time) {

//        x = -jump_extension*sin(jump_angle*PI/180);
			SpeedLimit = 8000;
				x = -jump_extension*cos(jump_angle*PI/180);
        y =  jump_extension*sin(jump_angle*PI/180);

        CartesianToTheta(1.0);

        // 使用高刚度和低阻尼执行跳转
				ChangeTheGainOfPID(8,0.0f,200,0.0f);
			  SetCoupledPosition(0);
			  SetCoupledPosition(1);
			  SetCoupledPosition(2);
			  SetCoupledPosition(3);

    } else if (tt >= prep_time + launch_time && tt < prep_time + launch_time + fall_time) {

				x = 17.3*cos((jump_angle+15)*PI/180);
        y = 17.3*sin((jump_angle+15)*PI/180);

        CartesianToTheta(1.0);
        //使用低刚度和大量的阻尼来处理下降
				ChangeTheGainOfPID(8,0.1,150,0);
			  SetCoupledPosition(0);
			  SetCoupledPosition(1);
			  SetCoupledPosition(2);
			  SetCoupledPosition(3);

    } else if (tt >= prep_time + launch_time + fall_time && tt < prep_time + launch_time + fall_time + streching_time) {
			
				x = 17.3*cos((jump_angle+15)*PI/180);
        y = 17.3*sin((jump_angle+15)*PI/180);

        CartesianToTheta(1.0);
        //使用低刚度和大量的阻尼来处理下降
			ChangeTheGainOfPID(8,0.1,70.0f,0.1f);
			  SetCoupledPosition(0);
			  SetCoupledPosition(1);
			  SetCoupledPosition(2);
			  SetCoupledPosition(3);
		
		}
		else {
			
				state = REALSE;
				ChangeTheGainOfPID(8,0.1,70,0.22);
    }

}

